/*
 * BSpline_6.c
 *
 *  Created on: Oct 15, 2011
 *      Author: yifili09
 */

#include "stdio.h"
#include "Point/point.h"
#include <complex.h>
#include <math.h>
#define BSPLINE //[BSPLINE , STOPANDWAIT , TESTSTATIC]

//#define DIVISON 20
//float splinex[DIVISON - 1];
//float spliney[DIVISON - 1];
#ifdef BSPLINE
#define KV 140
#define L 0.2
#define BMATH
point *pStart, *pEnd;
void newSpline(point *p2, point *p5, int n);
void bspline(float _Complex Z, int n);

int main() {
	float de = sinf(-45);
	printf("HelloWorld %f", de);

	pStart = pointAlloc(0.5, 0.5, 180); // 1 + 1*I with 45 Degree
	pEnd = pointAlloc(1.0, 2.0, 0); // 4 + 7*I with -45 Degree

	newSpline(pStart, pEnd, 10); //Create the 6 CheckPoint

	free(pStart);
	free(pEnd);
	return 0;
}

void newSpline(point *p2, point *p5, int n) {

#ifdef BMATH
	float rP = hypotf((p5->x - p2->x), (p5->y - p2->y)) / 3.0;
#endif

#ifdef BMATH
	point ZP[6];
#endif

#ifdef BMATH
	ZP[0].x = p2->x - rP * p2->x * cosf(p2->Theta); //45
	ZP[0].y = p2->y - rP * p2->y * sinf(p2->Theta);
	ZP[1].x = p2->x;
	ZP[1].y = p2->y;
	ZP[2].x = p2->x + rP * p2->x * cosf(p2->Theta);
	ZP[2].y = p2->y + rP * p2->y * sinf(p2->Theta);
#endif

#ifdef BMATH
	ZP[3].x = p5->x - rP * p5->x * cosf(p5->Theta);
	ZP[3].y = p5->y - rP * p5->y * sinf(p5->Theta);
	ZP[4].x = p5->x;
	ZP[4].y = p5->y;
	ZP[5].x = p5->x + rP * p5->x * cosf(p5->Theta);
	ZP[5].y = p5->y + rP * p5->y * sinf(p5->Theta);
#endif

#ifdef BMATH
	point tempAP[4];
	point splineCP[31];
#endif
	int count = 0;
	float t = 0;
	int i = 0;
	int var = 0;
#ifdef BMATH
	splineCP[0].x = p2->x;
	splineCP[0].y = p2->y;
#endif

#ifdef BMATH
	printf("****BASIC****************\n");
	for (var = 0; var < 3; ++var) {

		tempAP[0].x = (-ZP[var].x + 3 * ZP[var + 1].x - 3 * ZP[var + 2].x
				+ ZP[var + 3].x) / 6.0;
		tempAP[1].x = (3 * ZP[var].x + -6 * ZP[var + 1].x + 3 * ZP[var + 2].x)
				/ 6.0;
		tempAP[2].x = (-3 * ZP[var].x + 3 * ZP[var + 2].x) / 6.0;
		tempAP[3].x = (ZP[var].x + 4 * ZP[var + 1].x + ZP[var + 2].x) / 6.0;

		tempAP[0].y = (-ZP[var].y + 3 * ZP[var + 1].y - 3 * ZP[var + 2].y
				+ ZP[var + 3].y) / 6.0;
		tempAP[1].y = (3 * ZP[var].y + -6 * ZP[var + 1].y + 3 * ZP[var + 2].y)
				/ 6.0;
		tempAP[2].y = (-3 * ZP[var].y + 3 * ZP[var + 2].y) / 6.0;
		tempAP[3].y = (ZP[var].y + 4 * ZP[var + 1].y + ZP[var + 2].y) / 6.0;

		for (i = 1; i < n + 1; i++) {
			t = (float) i / n;
			count = var * n + i;
			splineCP[count].x = (tempAP[2].x
					+ t * (tempAP[1].x + t * tempAP[0].x)) * t + tempAP[3].x;

			splineCP[count].y = (tempAP[2].y
					+ t * (tempAP[1].y + t * tempAP[0].y)) * t + tempAP[3].y;

			/***** Check the splineComplex based on Point ********/
			printf("The splineCP[%d]: %f + %f\n", count, splineCP[count].x,
					splineCP[count].y);
			/*************************************/
		}
	}
#endif

#ifdef BMATH
	point dZP[30];
	float vP[30];
	float thP[30];
	float uP[29];

#endif

#ifdef BMATH
	float d1 = 0;
	float d2 = 0;
	printf("****BASIC****************\n");
	for (var = 0; var < 30; ++var) {
		dZP[var].x = splineCP[var + 1].x - splineCP[var].x;
		dZP[var].y = splineCP[var + 1].y - splineCP[var].y;

		vP[var] = hypotf(dZP[var].x, dZP[var].y); //Here needs the sampling Time Interval dZ/T
		thP[var] = atan2f(dZP[var].y,dZP[var].x);
		/*if (var > 1 ) {
			d1 = atan2f((splineCP[var + 1].y-splineCP[var].y), (splineCP[var + 1].x - splineCP[var].x));

			d2 = atan2f((splineCP[var].y - splineCP[var-1].y), (splineCP[var].x -splineCP[var-1].x));
			thP[var] = d1 - d2; //radium
		}else {
			thP[var] = atan2f(dZP[var].y,dZP[var].x);
		}


		if (var == 0) {
			d1 = splineCP[0].Theta;
		}else
			d1 = atan2l(splineCP[var].y,splineCP[var].x);
		d2 = atan2l(splineCP[var+1].y,splineCP[var+1].x);

		thP[var] = d2 - d1;*/

		//thP[var] = atan2f(dZP[var].y,dZP[var].x);
		/****************************************************/
		//printf("The vP[%d] is %f\n", var, vP[var]);
		//printf("The thP[%d] is %f\n", var, thP[var]);
		/****************************************************/
	}

	printf("***************************************\n");
	for (var = 0; var < 29; ++var) {

		uP[var] = thP[var + 1] - thP[var];

		if (uP[var] > PI) { //[-PI,PI]
			uP[var] = uP[var] - 2 * PI;
		} else if (uP[var] < -PI) {
			uP[var] = uP[var] + 2 * PI;
		}

	}
	printf("***************************************\n");
#endif

#ifdef BMATH
	float wR_P[29];
	float wL_P[29];

	printf("****BASIC****************\n");
	for (var = 0; var < 29; ++var) {

		wR_P[var] = (float) (KV * (vP[var] + (float) (L / 2.0) * uP[var]))
				+ 128;
		wL_P[var] = (float) (KV * (vP[var] - (float) (L / 2.0) * uP[var]))
				+ 128;
		printf("The Speed Set for Point[%d] is : (%f,%f)\n", var, wL_P[var],
				wR_P[var]);

	}

#endif
}

#endif

#ifdef STOPANDWAIT


#define DS_L	246.5509/30 - 1.1527
#define DS_R	246.5509/2 - 1.1527
int main(){

	float y_L = 0;
	float y_R	= 0;

//	for (;;) {
		y_L = DS_L;
		y_R = DS_R;

		printf("y_L:%d , y_R:%d",(int)y_L,(int)y_R);

		printf()
//	}




return 0;
}

#endif


#ifdef TESTSTATIC
static int j;

void fun1();
void fun2();

int main(){

	float _Complex i ;

	int var = 0;
	for (var = 0; var < 10; ++var) {
		fun1();
		fun2();
	}

	return 1;
}

void fun1(){
	static int i = 0;
	i++;
}

void fun2(){
	j = 0;
	j++;
}




#endif
